﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace DGEMS
{
    static class TunerUtils
    {
        internal static double FindFundamentalFrequency(double[] input, int sampleRate, double minFreq, double maxFreq)
        {
            double[] FFTvalues = FFT.Calculate(input);

            int usefulMinValue = Math.Max(0,
                (int)(minFreq * FFTvalues.Length / sampleRate));
            int usefulMaxValue = Math.Min(FFTvalues.Length,
                (int)(maxFreq * FFTvalues.Length / sampleRate) + 1);

            // find peaks in the FFT 
            const int peakCount = 3;
            int[] peakIndices;
            peakIndices = FindPeaks(FFTvalues, usefulMinValue, usefulMaxValue - usefulMinValue,
                peakCount);

            if (Array.IndexOf(peakIndices, usefulMinValue) >= 0)
            {
                // No Sound detected
                return 0;
            }

            const int offset = 0;
            int length = (int)(sampleRate / minFreq);

            // Find peaks with smaller difference
            double minPeakValue = Double.PositiveInfinity;
            int peakIndex = 0;
            int optimalInterval = 0;
            for (int i = 0; i < peakIndices.Length; i++)
            {
                int index = peakIndices[i];
                int intervalStart = FFTvalues.Length / (index + 1);
                int intervalEnd = FFTvalues.Length / index;
                int interval;
                double peakValue;
                
                ScanIntervals(input, offset, length,
                    intervalStart, intervalEnd, out interval, out peakValue);

                if (peakValue < minPeakValue)
                {
                    minPeakValue = peakValue;
                    peakIndex = index;
                    optimalInterval = interval;
                }
            }

            return (double)sampleRate / optimalInterval;
        }

        private static void ScanIntervals(double[] x, int peakIndex, int length,
            int intervalMin, int intervalMax, out int optimalInterval, out double optimalValue)
        {
            optimalValue = Double.PositiveInfinity;
            optimalInterval = 0;

            const int maxSteps = 30;
            int steps = intervalMax - intervalMin;
            if (steps > maxSteps)
                steps = maxSteps;
            else if (steps <= 0)
                steps = 1;

            // Find smallest distance
            for (int i = 0; i < steps; i++)
            {
                int interval = intervalMin + (intervalMax - intervalMin) * i / steps;

                double sum = 0;
                for (int j = 0; j < length; j++)
                {
                    double diff = x[peakIndex + j] - x[peakIndex + j + interval];
                    sum += diff * diff;
                }
                if (optimalValue > sum)
                {
                    optimalValue = sum;
                    optimalInterval = interval;
                }
            }
        }

        private static int[] FindPeaks(double[] values, int index, int length, int peaksCount)
        {
            double[] peakValues = new double[peaksCount];
            int[] peakIndices = new int[peaksCount];

            for (int i = 0; i < peaksCount; i++)
            {
                peakValues[i] = values[peakIndices[i] = i + index];
            }

            // find min value
            double minStoredPeak = peakValues[0];
            int minIndex = 0;
            for (int i = 1; i < peaksCount; i++)
            {
                if (minStoredPeak > peakValues[i])
                {
                    minStoredPeak = peakValues[minIndex = i];
                }
            }

            for (int i = peaksCount; i < length; i++)
            {
                if (minStoredPeak < values[i + index])
                {
                    
                    peakValues[minIndex] = values[peakIndices[minIndex] = i + index];

                   
                    minStoredPeak = peakValues[minIndex = 0];
                    for (int j = 1; j < peaksCount; j++)
                    {
                        if (minStoredPeak > peakValues[j])
                        {
                            minStoredPeak = peakValues[minIndex = j];
                        }
                    }
                }
            }

            return peakIndices;
        }
    }
}
